MdlPts = zeros(2,5);
% MdlPts(1,:) = [0 a a 0 d+c]; 
% MdlPts(2,:) = [b b 0 0 b-d-c];
MdlPts(1,:) = [269 213 351 417 295]; 
MdlPts(2,:) = [220 354 393 272 275];

X = MdlPts;

% ptSample = ptSampleR(:,:,idx);

% ptSample = [114   502   508   125   222; 439   441    71    56   340];
% ptSample = [193 343 492 348 274; 316 440 317 167 317];
% ptSample = [544   156    78   220   222
%      4   156   475   407   158];

ptSample = [271 215 353 420 296 
     219 355 393 272 272];



x = ptSample;

A = zeros(size(MdlPts,2)*2,9);
%     B = zeros(size(MdlPts,2)*2,1);

for i=1:size(MdlPts,2),
    A((i-1)*2+1,1:9)= [-X(1,i) -X(2,i) -1     0       0      0  x(1,i)*X(1,i) x(1,i)*X(2,i) x(1,i)];
    A((i-1)*2+2,1:9)= [    0      0     0 -X(1,i) -X(2,i)   -1  x(2,i)*X(1,i) x(2,i)*X(2,i) x(2,i)];
end


[U,S,V] = svd(A);

Hv = V(:,9);
H = [Hv(1:3,1)'; Hv(4:6,1)'; Hv(7:9,1)'];

% ret = H;
% modeli = homography(model, ret);
% plotmodel(modeli);

% hold on 
% plotmodel(model);
% Hv = pinv(A)*B; % Hv: H stacked in column vector
% H = [Hv(1:3,1)'; Hv(4:6,1)'; Hv(7:9,1)'];

 X1 = [X; 1 1 1 1 1];
 
 for i=1:5,
    x1(1,i) = (H(1,:)*X1(:,i))/(H(3,:)*X1(:,i));
    x1(2,i) = (H(2,:)*X1(:,i))/(H(3,:)*X1(:,i));
    x1(3,i) = 1;
 end
 
 hold on
% x11(1,:) = x1(1,:)./x1(3,:);
% x11(2,:) = x1(2,:)./x1(3,:);
% x11(3,:) = x1(3,:)./x1(3,:);

 plot(x1(1,:),x1(2,:),'r*',x(1,:),x(2,:),'k*')
